/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <iostream>

#include "ccamtransform.h"
#include "error/cerror.h"

using namespace std;

void CCamTransform::Reserve() {
	//intrinsic.create(3, 3);
	//distortion.create(1, 4);
	//rot_vect.create(3, 1);
	//rot_mat.create(3, 3);
	//trans_vect.create(3, 1);
	history_length = 5;
}

CCamTransform::~CCamTransform() {

}


/*!
    \fn CCamTransform::CalibrateIntrisictParam(IplImage * img, CvSize pattern_size, double h)
 */
bool CCamTransform::CalibrateIntrisictParams(CImageSource& src, cv::Size psize, double h, bool calc_distortion_tang) {
	int  board_found, nviews;
	cv::Mat img;
	
	cout << "Введите колличество сцен:" << std::endl;
	cin  >> nviews;
	//nviews = 2;
	
	vector<vector<cv::Point2f> > img_points;
	vector<vector<cv::Point3f> > obj_points;
	vector<cv::Mat> rvecs;
	vector<cv::Mat> tvecs;
	
	cv::namedWindow("Calibration", 1);
	
	for(int k = 0; k < nviews; k++) {
		std::cout << "Захват сцены " << k  << std::endl;

		img_points.push_back(vector<cv::Point2f>());
		obj_points.push_back(vector<cv::Point3f>());
		do {
			img = cv::Mat(src.GrabFrame());
			
			board_found = cv::findChessboardCorners(cv::Mat(img), psize, img_points.back());
			
			if (board_found) std::cout << "Калибровочная структура опознана." << std::endl;
			else std::cout << "Калибровочная структура НЕ опознана." << std::endl;
			
			if (board_found) cv::drawChessboardCorners(img, psize, img_points.back(), board_found);
			cv::imshow("Calibration", img);
			
			std::cout << "Нажмите ENTER." << std::endl;
			cv::waitKey(0);
		} while (!board_found);
			
		for(int i = 0; i < img_points.back().size(); i++) {
			obj_points.back().push_back(cv::Point3f(h*(i % psize.width), h*(i / psize.width), 0.0));
			//cout << img_points.back()[i] << " -> " << obj_points.back()[i] << endl;
		}
	}
	
	//TODO Destruction of window.
	//cv::destroyWindow("Calibration");
	cv::calibrateCamera(obj_points, img_points, img.size(), intrinsic, distortion, rvecs, tvecs, 0);
	
	rot_vect = rvecs.front();
	trans_vect = tvecs.front();
	cv::Rodrigues(rot_vect, rot_mat);
	
	std::cout << "Калибровка завершена." << std::endl;

	return board_found;
}


/*!
    \fn CCamTransform::SaveIntrinsicParams(const char * file)
 */
void CCamTransform::SaveIntrinsicParams(const char * file) const {
	//TODO Rervrite in o-o way.
	CvFileStorage * fs = cvOpenFileStorage(file, 0, CV_STORAGE_WRITE );
	if (!fs) throw Errors::CError("CCamTransform: Unable open file.");
	CvMat tmp = intrinsic;
	cvWrite(fs, "intrinsic", (const void*) &tmp, cvAttrList(0,0));
	tmp = distortion;
	cvWrite(fs, "distortion", (const void*) &tmp, cvAttrList(0,0));
	cvReleaseFileStorage( &fs );
}


/*!
    \fn CCamTransform::LoadIntrinsicParam(const char * file)
 */
void CCamTransform::LoadIntrinsicParams(const char * file)
{
	//TODO Rervrite in o-o way.
    CvFileStorage * fs = cvOpenFileStorage(file, 0, CV_STORAGE_READ);
	if (!fs) throw Errors::CError("CCamTransform: Unable open file.");
	CvMat * tmp = (CvMat *) cvReadByName(fs, 0, "intrinsic");
	if (!tmp) throw Errors::CError("CCamTransform: Bad itrinsic params file.");
	intrinsic = cv::Mat(tmp, true);
	cvReleaseMat(&tmp);
	tmp = (CvMat *) cvReadByName(fs, 0, "distortion");
	if (!tmp) throw Errors::CError("CCamTransform: Bad itrinsic params file.");
	distortion = cv::Mat(tmp, true);
	cvReleaseMat(&tmp);
	cvReleaseFileStorage(&fs);
}

/*!
    \fn CCamTransform::SaveTransform( SetDistortionModeconst char * file)
 */
void CCamTransform::SaveTransform(const char * file) const {
	//TODO Rervrite in o-o way.
	CvFileStorage * fs = cvOpenFileStorage(file, 0, CV_STORAGE_WRITE );
	if (!fs) throw Errors::CError("CCamTransform: Unable open file.");
	CvMat tmp = CvMat(rot_vect);
	cvWrite(fs, "rot_vect", (const void*) &tmp, cvAttrList(0,0));
	tmp = CvMat(rot_mat);
	cvWrite(fs, "rot_mat", (const void*) &tmp, cvAttrList(0,0));
	tmp = CvMat(trans_vect);
	cvWrite(fs, "trans_vect", (const void*) &tmp, cvAttrList(0,0));
	cvReleaseFileStorage( &fs );
}


/*!
    \fn CCamTransform::LoadTransform(const char * file)
 */
void CCamTransform::LoadTransform(const char * file)
{
	//TODO Rervrite in o-o way.
	CvFileStorage * fs = cvOpenFileStorage(file, 0, CV_STORAGE_READ);
	if (!fs) throw Errors::CError("CCamTransform: Unable open file.");
	
	CvMat * tmp = (CvMat *) cvReadByName(fs, 0, "rot_vect");
	if (!tmp) throw Errors::CError("CCamTransform: Bad transform params file.");
	rot_vect = cv::Mat(tmp,  true);
	cvReleaseMat(&tmp);
	
	tmp = (CvMat *) cvReadByName(fs, 0, "rot_mat");
	if (!tmp) throw Errors::CError("CCamTransform: Bad transform params file.");
	rot_mat = cv::Mat(tmp,  true);
	cvReleaseMat(&tmp);
	
	tmp = (CvMat *) cvReadByName(fs, 0, "trans_vect");
	if (!tmp) throw Errors::CError("CCamTransform: Bad transform params file.");
	trans_vect = cv::Mat(tmp,  true);
	cvReleaseMat(&tmp);
	
	cvReleaseFileStorage(&fs);
}

/*!
    \fn CCamTransform::CalcTransform()
 */
void CCamTransform::CalcTransform() {
	if (i_points.size() < 4) throw Errors::CBadArgError();
	
	cv::solvePnP(o_points, i_points, intrinsic, distortion, rot_vect, trans_vect, false);
	cv::Rodrigues(rot_vect, rot_mat);
}

bool CCamTransform::RecalcTransform() {
	if (i_points.size() < 4) return false;
		
	cv::solvePnP(o_points, i_points, intrinsic, distortion, rot_vect, trans_vect, true);
	cv::Rodrigues(rot_vect, rot_mat);
}

void CCamTransform::RemoveDistortion(const cv::Mat& src, cv::Mat& dst) const {
	cv::undistort(src, dst, intrinsic, distortion);
}

/*!
    \fn CCamTransform::ImgToObj(const cv::Point3f& obj)
 */
cv::Point2f CCamTransform::ProjectPoint(const cv::Point3f& obj) const {
	vector<cv::Point3f> o_point;
	vector<cv::Point2f> i_point;
	
	o_point.push_back(obj);
	cv::projectPoints(o_point, rot_vect, trans_vect, intrinsic, distortion, i_point);
	
	return i_point.front();
}

cv::Point3f CCamTransform::ReverseProjection(cv::Point2f pt, HPlane3f plane) const {
	double p;

	cv::Mat_<cv::Vec2f> img_pt(1, 1), cam_pt(1, 1);
	
	img_pt(0,0) = cv::Vec2f(pt.x, pt.y);
	cv::undistortPoints(img_pt, cam_pt, intrinsic, distortion);
	pt = cv::Point2f(cam_pt(0,0)[0], cam_pt(0,0)[1]);
	
	// 3 Reverse transform
	//plane in camera coordinates
	cv::Mat_<double> tmp1(3,1);
	cv::Mat_<double> tmp2(3,1);
	tmp1(0,0) = plane.A; tmp1(1,0) = plane.B; tmp1(2,0) = plane.C;  
	tmp2 = rot_mat * tmp1;
	plane.A = tmp2(0,0); plane.B = tmp2(1,0); plane.C = tmp2(2,0);
	plane.D -= tmp2.dot(trans_vect);
	//calculate position in camera coordinates
	p = - plane.D / (plane.A * pt.x + plane.B * pt.y + plane.C);
	tmp1(0, 0) = p * pt.x;  
	tmp1(1, 0) = p * pt.y;
	tmp1(2, 0) = p;
	//transform to object coordinates
	tmp2 = tmp1 - trans_vect;
	tmp1 = rot_mat.t()*tmp2;
	
	return cv::Point3f(tmp1(0,0), tmp1(1,0), tmp1(2,0));
}

void CCamTransform::PrintIParams() const {
	std::cout << "fx = " << intrinsic(0, 0) << std::endl;
	std::cout << "fy = " << intrinsic(1, 1) << std::endl;
	std::cout << "cx = " << intrinsic(0, 2) << std::endl;
	std::cout << "cy = " << intrinsic(1, 2) << std::endl;
	std::cout << "dist = ";
	for (int i = 0; i < 4; i++) std::cout << " " << distortion(0, i);
	std::cout << std::endl;
}

void CCamTransform::PrintTParams() const {
	std::cout << "t = (";
	for (int i = 0; i < 3; i++) std::cout << " " << trans_vect(i, 0);
	std::cout << " )*" << std::endl;
	
	std::cout << "rot = (";
	for (int i = 0; i < 3; i++) std::cout << " " << rot_vect(i, 0);
	std::cout << " )*" << std::endl;
	
	std::cout << "rot_mat = (" << std::endl;;
	for (int j = 0; j < 3; j++) {
		for (int i = 0; i < 3; i++)  std::cout << " " << rot_mat(j, i);
		std::cout << std::endl;
	}
	std::cout << " )" << std::endl;
}
